//Michele Gregori 2009 miclegr@gmail.com

#include "MDualQuaternion.h"

MDualQuaternion::MDualQuaternion(){
	real[0]=0;
	real[1]=0;
	real[2]=0;
	real[3]=0;
	dual[0]=0;
	dual[1]=0;
	dual[2]=0;
	dual[3]=0;
}

MDualQuaternion MDualQuaternion::operator+=(MDualQuaternion toSum){
	real[0]+=toSum.real[0];
	real[1]+=toSum.real[1];
	real[2]+=toSum.real[2];
	real[3]+=toSum.real[3];
	dual[0]+=toSum.dual[0];
	dual[1]+=toSum.dual[1];
	dual[2]+=toSum.dual[2];
	dual[3]+=toSum.dual[3];
	return *this;
}

MDualQuaternion MDualQuaternion::operator-=(MDualQuaternion toSub){
	real[0]-=toSub.real[0];
	real[1]-=toSub.real[1];
	real[2]-=toSub.real[2];
	real[3]-=toSub.real[3];
	dual[0]-=toSub.dual[0];
	dual[1]-=toSub.dual[1];
	dual[2]-=toSub.dual[2];
	dual[3]-=toSub.dual[3];
	return *this;		
}

MDualQuaternion MDualQuaternion::operator *(double scale){
	MDualQuaternion multiplyed;
	multiplyed.real[0]=real[0]*scale;
	multiplyed.real[1]=real[1]*scale;
	multiplyed.real[2]=real[2]*scale;
	multiplyed.real[3]=real[3]*scale;
	multiplyed.dual[0]=dual[0]*scale;
	multiplyed.dual[1]=dual[1]*scale;
	multiplyed.dual[2]=dual[2]*scale;
	multiplyed.dual[3]=dual[3]*scale;
	return multiplyed;
}


void MDualQuaternion::operator=(MDualQuaternion dq){
	real[0]=dq.real[0];
	real[1]=dq.real[1];
	real[2]=dq.real[2];
	real[3]=dq.real[3];
	dual[0]=dq.dual[0];
	dual[1]=dq.dual[1];
	dual[2]=dq.dual[2];
	dual[3]=dq.dual[3];
}

MDualQuaternion MDualQuaternion::normalize(){
	MDualQuaternion norm;
	double magn=sqrt(real[0]*real[0]+real[1]*real[1]+real[2]*real[2]+real[3]*real[3]);
	norm.real[0]=real[0]/magn;
	norm.real[1]=real[1]/magn;
	norm.real[2]=real[2]/magn;
	norm.real[3]=real[3]/magn;
	norm.dual[0]=dual[0]/magn;
	norm.dual[1]=dual[1]/magn;
	norm.dual[2]=dual[2]/magn;
	norm.dual[3]=dual[3]/magn;
	return norm;
}

MDualQuaternion::MDualQuaternion(const MMatrix& mat){
	if(mat[0][0]+mat[1][1]+mat[2][2]>0.0){
		double t=+mat[0][0]+mat[1][1]+mat[2][2]+1.0;
		double s=(1/sqrt(t))*0.5;
		real[3]=s*t;
		real[2]=(mat[0][1]-mat[1][0])*s;
		real[1]=(mat[2][0]-mat[0][2])*s;
		real[0]=(mat[1][2]-mat[2][1])*s;
	}else if(mat[0][0]>mat[1][1]&&mat[0][0]>mat[2][2]){
		double t=+mat[0][0]-mat[1][1]-mat[2][2]+1.0;
		double s=(1/sqrt(t))*0.5;
		real[0]=s*t;
		real[1]=(mat[0][1]+mat[1][0])*s;
		real[2]=(mat[2][0]+mat[0][2])*s;
		real[3]=(mat[1][2]-mat[2][1])*s;
	}else if(mat[1][1]>mat[2][2]){
		double t=-mat[0][0]+mat[1][1]-mat[2][2]+1.0;
		double s=(1/sqrt(t))*0.5;
		real[1]=s*t;
		real[0]=(mat[0][1]+mat[1][0])*s;
		real[3]=(mat[2][0]-mat[0][2])*s;
		real[2]=(mat[1][2]+mat[2][1])*s;
	}else{
		double t=-mat[0][0]-mat[1][1]+mat[2][2]+1.0;
		double s=(1/sqrt(t))*0.5;
		real[2]=s*t;
		real[3]=(mat[0][1]-mat[1][0])*s;
		real[0]=(mat[2][0]+mat[0][2])*s;
		real[1]=(mat[1][2]+mat[2][1])*s;
	}
	dual[3]=0.5*(-mat[3][0]*real[0]-mat[3][1]*real[1]-mat[3][2]*real[2]);
	dual[0]=0.5*(+mat[3][0]*real[3]-mat[3][1]*real[2]+mat[3][2]*real[1]);
	dual[1]=0.5*(+mat[3][0]*real[2]+mat[3][1]*real[3]-mat[3][2]*real[0]);
	dual[2]=0.5*(-mat[3][0]*real[1]+mat[3][1]*real[0]+mat[3][2]*real[3]);
}

MMatrix MDualQuaternion::asMatrix(){
	MMatrix mat;
	double x2=real[0]+real[0]; double y2=real[1]+real[1];double z2=real[2]+real[2];
	double yy2=real[1]*y2; double xy2=real[0]*y2; double xz2=real[0]*z2; double yz2=real[1]*z2;
	double zz2=real[2]*z2; double wz2=real[3]*z2; double wy2=real[3]*y2; double wx2=real[3]*x2; double xx2=real[0]*x2;
	mat[0][0]=-yy2-zz2+1.0; mat[0][1]=xy2+wz2; mat[0][2]=xz2-wy2;
	mat[1][0]=xy2-wz2; mat[1][1]=-xx2-zz2+1.0; mat[1][2]=yz2+wx2;
	mat[2][0]=xz2+wy2; mat[2][1]=yz2-wx2; mat[2][2]=-xx2-yy2+1.0;
	mat[3][0]=-2*dual[0]*real[1]+2*real[0]*dual[1]-2*dual[2]*real[3]+2*real[2]*dual[3];
	mat[3][1]=-2*dual[0]*real[2]+2*dual[1]*real[3]-2*real[1]*dual[3]+2*real[0]*dual[2];
	mat[3][2]=-2*dual[0]*real[3]+2*real[1]*dual[2]+2*real[0]*dual[3]-2*dual[1]*real[2];
	return mat;
}

void MDualQuaternion::mulPoint(MPoint &point){
	MDualQuaternion normalized=this->normalize();
	double cross1[4],cross2[4],cross3[4],pointVal[4];
	point.get(pointVal);
	this->cross(normalized.real,pointVal,cross1);
	cross1[0]+=normalized.real[3]*pointVal[0];cross1[1]+=normalized.real[3]*pointVal[1];cross1[2]+=normalized.real[3]*pointVal[2];
	this->cross(normalized.real,cross1,cross2);
	this->cross(normalized.dual,normalized.real,cross3);
	point.x+=2*cross2[0]+2*(normalized.real[3]*normalized.dual[0]-normalized.dual[3]*normalized.real[0]+cross3[0]);
	point.y+=2*cross2[1]+2*(normalized.real[3]*normalized.dual[1]-normalized.dual[3]*normalized.real[1]+cross3[1]);
	point.z+=2*cross2[2]+2*(normalized.real[3]*normalized.dual[2]-normalized.dual[3]*normalized.real[2]+cross3[2]);
}

void MDualQuaternion::cross(double vect1[4],double vect2[4],double (&crossed)[4]){
	crossed[0]=vect1[1]*vect2[2]-vect1[2]*vect2[1];
	crossed[1]=vect1[2]*vect2[0]-vect1[0]*vect2[2];
	crossed[2]=vect1[0]*vect2[1]-vect1[1]*vect2[0];
}

double MDualQuaternion::dot(MDualQuaternion dq){
	return (real[0]*dq.real[0]+real[1]*dq.real[1]+real[2]*dq.real[2]+real[3]*dq.real[3]);
}
